#include "swarm_avoid_client/swarm_computer.hpp"
#include <cmath>
#include <algorithm>
#include <iostream>

// 默认构造函数，初始化参数
swarm_controller::swarm_controller()
    : K_attract(0.4), K_repulse(15.0), repulse_radius(25.0),
      safe_distance(7.0), max_speed(5),
      K_attract2(25.0), repulse_radius2(30.0), repulse_exponent2(14.0),
      guidance_force_reduction(0.5), distance_threshold(25.0), repulse_exponent(4.0) {}

    //引力、排斥、排斥半径
    //安全距离、最大速度
    //引力2、排斥半径、排斥指数
    //制导力缩减、距离阈值、排斥指数
// 设置目标位置
void swarm_controller::set_goal(const Vector3d& goal) {
    this->goal = goal;
}

// 设置位置
void swarm_controller::set_positions(const Vector3d& current_position, const std::unordered_map<std::string, Vector3d>& other_positions) {
    this->current_position = current_position;
    this->other_positions = other_positions;
}

// 设置障碍物
void swarm_controller::set_obstacles(const std::unordered_map<int,Obstacle>& obstacles) {
    std::lock_guard<std::mutex> lock(obstacles_mutex_); // 加锁
    this->obstacles = obstacles;
}

// 添加障碍物
void swarm_controller::add_obstacle(const Obstacle obstacle, int index) {
    std::lock_guard<std::mutex> lock(obstacles_mutex_); // 加锁
    obstacles[index] = obstacle;
}

// 获取障碍物
Obstacle swarm_controller::get_obstacle(int index) {
    std::lock_guard<std::mutex> lock(obstacles_mutex_); // 加锁
    if (obstacles.find(index) != obstacles.end()) {
        return obstacles[index];
    }
    throw std::runtime_error("Obstacle not found");
}

bool swarm_controller::clearObstacle(int ObIndex){
    std::lock_guard<std::mutex> lock(obstacles_mutex_); // 加锁
    obstacles.erase(ObIndex);
    return true;
}

bool swarm_controller::ObstacleExist(int ObIndex){
    std::lock_guard<std::mutex> lock(obstacles_mutex_); // 加锁
    auto it = obstacles.find(ObIndex);
    if (it != obstacles.end()) {
        return true;
    }
    else{
        return false;
    }
}

// 设置参数
void swarm_controller::set_parameters(double K_attract, double K_repulse, double repulse_radius, 
                                       double safe_distance, double repulse_exponent,
                                       double max_speed, double K_attract2, 
                                       double repulse_radius2, double repulse_exponent2) {
    this->K_attract = K_attract;
    this->K_repulse = K_repulse;
    this->repulse_radius = repulse_radius;
    this->safe_distance = safe_distance;
    this->max_speed = max_speed;
    this->K_attract2 = K_attract2;
    this->repulse_radius2 = repulse_radius2;
    this->repulse_exponent2 = repulse_exponent2;
    this->repulse_exponent = repulse_exponent;
}

Vector3d swarm_controller::compute_force() {

    // 计算目标吸引力
    Vector3d attract_force = K_attract * (goal - current_position);

    // 计算与其他飞机的排斥力和修正引导力
    Vector3d formation_repulse(0, 0, 0);
    Vector3d guidance_force(0, 0, 0);

    // 计算与其他飞机的排斥力和引导力
    // for (size_t k = 0; k < other_positions.size(); k++) {
    for(auto &other_position:other_positions)
    {
        Vector3d relative_position = current_position - other_position.second;
        double dist = relative_position.norm();

        // 判断飞机之间的距离是否小于安全距离
        if (dist < safe_distance) {
            // 增加排斥力：飞机之间的距离越小，排斥力越大
            double repulse_factor = std::pow((safe_distance - dist), repulse_exponent);  // 使用更高的指数，增强排斥力
            formation_repulse += K_repulse * repulse_factor * relative_position.normalized();
            
            // 修正引导力的方向，使其远离其他飞机
            guidance_force -= K_attract2 * std::pow((safe_distance - dist), 3) * relative_position.normalized();
        }
    }

    {
        std::lock_guard<std::mutex> lock(obstacles_mutex_); // 加锁

        // 计算与障碍物的引导区域力
        for (const auto& obstacle : obstacles) {
            double dist = (current_position - obstacle.second.position).norm();

            std::cout<<"dist of obstacle is:"<<dist<<std::endl;
            
            if (dist < repulse_radius2 + obstacle.second.obstacle_radius) {
                Vector3d obstacle_to_drone = current_position - obstacle.second.position;
                Vector3d obstacle_to_drone_unit = obstacle_to_drone.normalized();
                Vector3d target_direction = goal - current_position;
                Vector3d target_direction_unit = target_direction.normalized();

                // 计算引导区域方向
                Vector3d perpendicular_vector = target_direction_unit.cross(obstacle_to_drone_unit);
                if (perpendicular_vector.norm() == 0) {
                    Vector3d base_vector(1, 0, 0);
                    if (base_vector == target_direction_unit) {
                        base_vector = Vector3d(0, 1, 0);
                    }
                    perpendicular_vector = target_direction_unit.cross(base_vector).normalized();
                }
                Vector3d guidance_direction = perpendicular_vector.cross(obstacle_to_drone_unit).normalized();
                double repulse_dist = repulse_radius2 + obstacle.second.obstacle_radius - dist;

                double angle = acos(target_direction_unit.dot(guidance_direction));
                if (angle < M_PI / 2) {
                    guidance_force += K_attract2 * std::pow(repulse_dist, repulse_exponent2) * guidance_direction;
                } else {
                    guidance_force -= K_attract2 * std::pow(repulse_dist, repulse_exponent2) * guidance_direction;
                }
            }
        }
    }

    // 方法2：根据目标方向与引导力的角度调整引导力的影响
    Vector3d target_direction = goal - current_position;
    Vector3d target_direction_unit = target_direction.normalized();
    double angle_to_target = acos(target_direction_unit.dot(guidance_force.normalized()));
    
    // 如果引导力和目标方向的夹角大于90度，减小引导力的影响
    if (angle_to_target > M_PI / 2) {
        guidance_force *= guidance_force_reduction;  // 使用类参数缩小引导力
    }

    // 动态调整引导力与目标吸引力的比例
    double distance_to_goal = sqrt(pow((goal(0) - current_position(0)),2)+pow((goal(1) - current_position(1)),2));

    // 计算总力
    Vector3d total_force = attract_force + guidance_force + formation_repulse;

    std::cout<<"attract_force:"<<attract_force<<std::endl;

    std::cout<<"guidance_force:"<<guidance_force<<std::endl;

    std::cout<<"formation_repulse:"<<formation_repulse<<std::endl;

    std::cout<<"goal distance!  "<<distance_to_goal<<std::endl;

    // 最终速度计算，限制最大速度
    double speed = std::min(total_force.norm(), max_speed);  // 限制最大速度

    std::cout<<"total_force.normalized() * speed:"<<total_force.normalized() * speed<<std::endl;

    return total_force.normalized() * speed;  // 返回单位速度方向
}
// source install/setup.bash